/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "3DPointStorage.h"

C3DPointStorage::C3DPointStorage(double sizeX, double sizeY, double sizeZ, double resolution){
	Nx = (uint64_t)(sizeX/resolution + 0.5);
	Ny = (uint64_t)(sizeY/resolution + 0.5);
	Nz = (uint64_t)(sizeZ/resolution + 0.5);
	sx = sizeX;
	sy = sizeY;
	sz = sizeZ;
	reso = resolution;
}

bool C3DPointStorage::indexToCoord(uint64_t index, lnm::TCoord3D &coord){
	double z = (((index / (Nx*Ny))*reso)) -  sz/2.0; //Get the Z level
	
	uint64_t iz = (index / (Nx*Ny));
	
	
	uint64_t xy = index%(Nx*Ny);
	//fprintf(stderr,"%ld %ld ",xy,iz);
	double x = ((xy%Nx)*reso) - sx/2.0;
	double y = ((xy/Nx)*reso) - sy/2.0;
	
	coord.set(x,y,z);
	return true;
}
bool C3DPointStorage::coordToIndex(lnm::TCoord3D &coord, uint64_t &index){
if((coord.y+sy/2.0)>sy || (coord.x + sx/2.0)>sx || (coord.z+sz/2.0)>sz ||
	(coord.y + sy/2.0)<0 || (coord.x + sx/2.0)<0 || (coord.z+sz/2.0)<0){
	return false;
	}
	
	index = (uint64_t)( floor((coord.x + sx/2.0)/reso +0.5))+ (uint64_t)(((coord.y + sy/2.0)/reso + 0.5))*Nx + 
					(uint64_t)((coord.z+sz/2.0)/reso + 0.5)*Nx*Ny;

	return true;
}

bool C3DPointStorage::setCoordPoint(lnm::TCoord3D &coord, lnm::RGB color){
	uint64_t key;
	
	if(coordToIndex(coord, key)){
		map<uint64_t,lnm::RGB>::iterator it;
		
		//it = points.find(key);
		
		//if(it == points.end()){
			//fprintf(stderr,"New element\n");
		//}else{
			//fprintf(stderr,"old element\n");
		//}
		
		points[key]=color;
	}else{
		return false;
	}

	return true;
}

bool C3DPointStorage::save(const char *fname){
	FILE *f = fopen(fname,"wt");
	map<uint64_t,lnm::RGB>::iterator it;
	lnm::TCoord3D coord;
	
	for(it = points.begin();it!=points.end();it++){
		indexToCoord( (it->first), coord);
		fprintf(f,"%.3f %.3f %.3f %d %d %d\n",coord.x,coord.y,coord.z, (it->second).r,(it->second).g,(it->second).b );
	}
	
	fclose(f);
	
	return true;
}

